
#include "config.h"
#include <SPI.h>
#include <RF22.h>
// Change according to the version
#if defined(ARDUINO) && ARDUINO >= 100
  #include "Arduino.h"
#else
  #include "WProgram.h"
#endif
#if !defined(PPM_OUT)
  #include <Servo.h>
#endif

// Pin Settings
#define greenLED 18
#define redLED 19
#define outPinPPM 8

#define details(name) (uint8_t*)&name,(uint8_t*)sizeof(name)

struct RECEIVE_DATA_STRUCTURE{
  //put your variable definitions here for the data you want to receive
  //THIS MUST BE EXACTLY THE SAME ON THE OTHER ARDUINO
  int AxisIn[CHANNELS];
};

struct TELE_DATA_STRUCTURE{
  byte RSSI;
  byte RxBad;
  float Alt;
};

RECEIVE_DATA_STRUCTURE rxdata;
TELE_DATA_STRUCTURE teledata;

#if !defined(PPM_OUTPUT)
  Servo MyServos[CHANNELS];
#endif

// Singleton instance of the radio
RF22 rf22;

long Now = 0, TimeOld = 0, TeleTimeOld = 0;     
long deadTime, deadTimeOld, badpackets, badpacketsOld;
byte rssi, cycleCount;

void setup() 
{  
  //Serial.begin(38400);
  //Serial.println("Starting initialization.");
  
  RF22Initialization();
  //Serial.println("RF22 initialized.");
  
  #if defined(PPM_OUTPUT)
    
    // Set default servo position to middle - 1500
    rxdata.AxisIn[0] = 3000;
    rxdata.AxisIn[1] = 3000;
    rxdata.AxisIn[2] = 3000;
    rxdata.AxisIn[3] = 3000;
    rxdata.AxisIn[4] = 3000;
    rxdata.AxisIn[5] = 3000;
    rxdata.AxisIn[6] = 3000;
    rxdata.AxisIn[7] = 3000;

    INIT_SERVO_DRIVER();
  #else
    servoInit();
  #endif
  delay(500);
  
  //Serial.println("Channels initialized.");
  rf22.waitAvailable();
  //Serial.println("RF22 is available.");
  rf22.recv(details(rxdata));
  //Serial.println("RF22 initial data received");
  //Serial.println("Initialization done");
}

void loop()
{
  Now = millis();

  if ((Now-TimeOld > refreshTime)) {
    TimeOld = Now;
    //Serial.println("0");
    deadTime = Now - deadTimeOld;
    
    if (deadTime > FAILSAFE_TIMEOUT)
    {
      digitalWrite(greenLED, LOW);
      digitalWrite(redLED, LOW);
      rssi = 0;
      //if no signal received for 200ms, set rssi to 0
    }
  }

  if (rf22.recv(details(rxdata))) 
  {
    deadTimeOld = Now;
    
    #if !defined(PPM_OUTPUT)
      writeServos();
    #endif
    
    /*
    for (byte i = 0; i < CHANNELS; i++ ){
     Serial.print(rxdata.AxisIn[i]);
     Serial.print(",");
     }
     Serial.println("");
    */
    
    cycleCount++;

    /* 
    Cycle 1: Read RSSI
    Cycle 2: Assemble and send tele packet
    Cycle 3-9: nothing
    Cycle 10: Loop back
    */
    switch(cycleCount){
      case(1):
        RSSI();
        break;

      case(2):
        teledata.RSSI = rssi;
        badpackets = rf22.RxBad();
        teledata.RxBad = abs(badpacketsOld-badpackets);
        badpacketsOld = badpackets;
      
        //Serial.print(teledata.RxBad);
        //Serial.print(',');
        //Serial.println(rssi);
        
        rf22.send((uint8_t*)&teledata, sizeof(teledata));
        rf22.waitPacketSent();
        break;

      case(10):
        cycleCount = 0;
        break;
    }
    //Serial.println("4");
  }
  else
  {
    //Serial.println("Receive Error");
  }

}

















